/**
 * @author Jerome Monceaux
 * @version 1.0
 * 
 * LocomotionSingle : Compute motor speed on a single motor according
 * to a dynamic target.
 * It need to be configure with a class instance inherited from MotorInterface.
 * Geometric parameter of wheel are needed to convert encoder value to 
 * meters and radian.
 * International Metric System is systematically used. 
 **/
package org.hh.locodo;

import ioio.lib.api.exception.ConnectionLostException;
import android.util.Log;

public class LocomotionSingle extends OdometrySingle {

	public LocomotionSingle(double encoderWheelPerimeter,
			int encoderTickPerRotation) {
		super(encoderWheelPerimeter, encoderTickPerRotation);
	  // Create the odometric engine. It needs encoder wheel perimeter, 
	  // and the number of tick per rotation of the encoder wheel
	  
	  fPid.P = 1.;
	  fPid.I = 0.;
	  fPid.D = 0.;
	  fPid.iLimit = 0.;
	  
	  // initialization
	  fMotor = null;
	  
	  fMaxSpeed = 1.0;
	  
	  fOldDelta = 0.;
	  
	  // default robot precision
	  fPrecision = 0.01f;
	  
	  // set current position to 0.
	  fPosition = 0.;

	  // initial target Reached
	  fTarget = fPosition;
	  fTargetReached = true;
	  
	  
	  // default values of acceleration limit
	  fAccLimit = 0.015; //meter/cycle 
	  
	  // stop signal initialization
	  fRegularFreeze = false;
	  fEmergencyFreeze = false;
	  
	  // ramp enable initialization
	  fRampEnable = true;
	  
	  fLastCurrEncoderValue = 0;
	  
	};
	

  /**
   * set right and left motor instance
   * @param motorLeft ODOMotorInterface's pointer of the left motor 
   * @param motorRight ODOMotorInterface's pointer of the right motor 
   */
  public void setMotor( MotorInterface motor){
  	fMotor = motor;
	};
  
	public int getRealSpeed(){ return fRealSpeed;};
	
  /**
   * update. Must frequent to keep high precision
   * @throws ConnectionLostException 
   */
  public void update() throws ConnectionLostException{
  	
  	if( fMotor == null ){
  		return;
  	}
     
    //call motor controller update
    fMotor.update();
    
    // get encoder value
    // take care that encoder are in opposite side. Conventionally let's inverse right encoder value.
    int currEncoderValue = fMotor.getEncoderValue();
    
    /*// filter incoherent values
    if( Math.abs(currEncoderValue - fLastCurrEncoderValue) > 50 )
    {
      //Log.i("HH", "ODOLocomotionSingle: get encoder " + Integer.toString(currEncoderValue)
      //						+ "last encoder "  + Integer.toString(fLastCurrEncoderValue) );
      Log.e("HH", "Encodeur Value rejected");
      return;
    }*/
    
    fRealSpeed = currEncoderValue - fLastCurrEncoderValue;
    fLastCurrEncoderValue = currEncoderValue;

    // Update the position
    fPosition = updatePosition( currEncoderValue );
    // Log.d("HH", "position:" + Double.toString( fPosition) );
    
    
    // get the difference between current position and target
    double target = getRelativeTargetPosition( fTarget );
    
    // if the target already reached return
    if( fTargetReached )
      return;
    
    // Determine if the target is reach
    if( Math.abs( target ) < fPrecision )
    {  
      fTargetReached = true;
      Log.i("HH", "target reached");
      fMotor.setSpeed( 0.0f );
      return;
    }else
    {  
      fTargetReached = false;
    }
    
    // if the stop command has been called
    // overwrite target distance
    if( fRegularFreeze )
    {
      target = 0.f;
      fRegularFreeze = false;
    }

    // apply the acceleration limitation
    if( fRampEnable ) target = xAccLimit( target );
    
    // send new command to the pid
    fPid.setCurrValue( target, 1 );

    // compute pid 
    double newSpeed = fPid.update( );

    // Mixes angle command and straight command
    double speed = newSpeed;

    // energencyStop method has been called
    if( fEmergencyFreeze){
    	Log.i("HH", "Emergency freeze has been called. LocomotionSingle stop!");
    	fMotor.setSpeed(0.);
    }else
    {
      // callback motor controller to send the speed command
      //ODO_DEBUG_LOCOSINGLE( newSpeed  << "\t" << currEncoderValue << "\t" << target );
      fMotor.setSpeed( fMaxSpeed * speed );
    }
	}

  /**
   * set current Target.
   * @param target new Target to be reached. <BR>
   * A specific case is done if target.x and target.y are set to 0, 
   * then we consider target as a angle target, in this case X and Y coordinates are ignored.
   * d is systematically ignored
   */
  public void setTarget( double target ){
    fTarget = target;
    fTargetReached = false;
    Log.i( "HH", "new target is " + Double.toString(fTarget) ); 
	}

  /**
   * @return the current robot target
   */
  public double getTarget(){ 
  	return fTarget; 
  };
  
  /**
   * @return true if target position is under the defined precision
   */
  public boolean isTargetReached(){ 
  	return fTargetReached;
  };
  
  
  /*======================
   * accessors
  ====================== */
  /**
   * accessors to set the PID parameters of the distance control
   */ 
  /** @param p set the proportional coefficient of the distance pid regulator */
  public void setP( double p) { fPid.P = p; };
  
  /** @param i set the integral coefficient of the distance pid regulator */
  public void setI( double i) { fPid.I = i; };
  
  /** @param d set the derivative coefficient of the distance pid regulator */
  public void setD( double d) { fPid.D = d; };
  
  /** @return the current proportional coefficient of the distance pid regulator */
  public double getP()        { return fPid.P; };
  
  /** @return the current integral coefficient of the distance pid regulator */
  public double getI()        { return fPid.I; };
  
  /** @return the current derivative coefficient of the distance pid regulator */
  public double getD()        { return fPid.D; };
  

  /** @param pDistance Desire angle precision of the angle regulator */
  public void setPrecision( double precision ){ fPrecision = precision; };
  
  /** @return the desire distance precision of the angle regulator */
  public double getPrecision( ){ return fPrecision; };
  
  /** @param maxSpeed Maximum motor speed */
  public void setMaxSpeed( double maxSpeed ) { fMaxSpeed = maxSpeed; };
  
  /** @return Current maximum motor speed */
  public double getMaxSpeed( ) { return fMaxSpeed; };

  /** @param accLimit Distance target acceleration limit */
  public void setAccLimit(double accLimit){ fAccLimit = accLimit; };
  
  /** @return the current distance target acceleration limit */
  public double getAccLimit(){ return fAccLimit;};

  
  /**
   * Stop with respect of the limit acceleration criteria.
   * These methods needs to be called befaire each update !
   */
  public void freeze(){ fRegularFreeze = true; };
    
  /**
   * Stop the robot as soon as possible without respect 
   * of the acceleration limit.
   */
  public void emergencyFreeze(){ fEmergencyFreeze = true; };
  
  /**
   * Release the robot from emergency freeze state.
   */ 
  public void emergencyUnfreeze(){ fEmergencyFreeze = false; };
  
  /**
   * Disable distance regulator acceleration ramp
   */
  public void setRampEnable( boolean onOff ) { fRampEnable = onOff; };
  
    
  /**
   * Apply a limit acceleration on the Distance pid regulator input. This limit is define by the user.
   */
  private double xAccLimit( double in ){
    double tmp = in;
    if( in < fOldDelta - fAccLimit)
      tmp = fOldDelta - fAccLimit;
    if( in > fOldDelta + fAccLimit)
      tmp = fOldDelta + fAccLimit;
    fOldDelta = tmp;
    return tmp;
	}

  /** pid parameters of the distance regulator */
  private PID fPid;
  
  /**
   * current position
   */
  private double fPosition;
  
  /**
   * current target
   */
  private double fTarget;
  
  /** Pointer to the left motor controller */
  private MotorInterface fMotor;
  
  // if the target is reached
  private boolean fTargetReached;
  
  // precision values (angle, distance)
  private volatile double fPrecision;
  
  // max speed
  private volatile double fMaxSpeed;
  
  // backup of the previous value. 
  // Use to apply ramp on the pid input data
  private double fOldDelta;
  
  // acceleration limits
  private volatile double fAccLimit;
  
  // stop signal
  private boolean fRegularFreeze;
  private boolean fEmergencyFreeze;
  
  // disable ramp signal
  private boolean fRampEnable;
  
  private int fLastCurrEncoderValue;
  private int fRealSpeed;
  
}
